/*
 * This file is part of the OpenKinect Project. http://www.openkinect.org
 *
 * Copyright (c) 2010 Brandyn White (bwhite@dappervision.com)
 *
 * This code is licensed to you under the terms of the Apache License, version
 * 2.0, or, at your option, the terms of the GNU General Public License,
 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
 * or the following URLs:
 * http://www.apache.org/licenses/LICENSE-2.0
 * http://www.gnu.org/licenses/gpl-2.0.txt
 *
 * If you redistribute this file in source form, modified or unmodified, you
 * may:
 *   1) Leave this header intact and distribute it under the same terms,
 *      accompanying it with the APACHE20 and GPL20 files, or
 *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
 *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
 * In all cases you must keep the copyright notice intact and include a copy
 * of the CONTRIB file.
 *
 * Binary distributions must follow the binary distribution requirements of
 * either License.
 */
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/shm.h>
#include <netinet/in.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <sys/wait.h>
#include <libfreenect.h>
#include <sys/time.h>
#include <signal.h>
#include <fcntl.h>
#include "ffmpegEnc/ffmpeg.h"
#include "record.h"
#include <pthread.h>
#include <assert.h>

int sockfd1,sockfd2,sockfd3,sockfd4, new_fd1,new_fd2,new_fd3,new_fd4;  // listen on sock_fd, new connection on new_fd
int freenect_oldAngle=0,accelCount=0,setAngle=0,rgbFrameCounter=0,depthFrameCounter=0,exprgb=0,expdepth=0;
char *out_dir;
int checkAngle=1;
volatile sig_atomic_t running = 1;
uint32_t last_timestamp = 0;
FILE *index_fp = NULL;

void sigchld_handler(int s){
	while(waitpid(-1, NULL, WNOHANG) > 0);
}

// get sockaddr, IPv4 or IPv6:
void *get_in_addr(struct sockaddr *sa){
	if (sa->sa_family == AF_INET) {
		return &(((struct sockaddr_in*)sa)->sin_addr);
	}
	return &(((struct sockaddr_in6*)sa)->sin6_addr);
}

double get_time(){
	struct timeval cur;
	gettimeofday(&cur, NULL);
	return cur.tv_sec + cur.tv_usec / 1000000.;
}

void handleAngleRequest(){
//printf("Inside handle angle req\n");
	int numbytes;
	char ack='z';
	if (((numbytes = recv(new_fd4, &ack, 1,0)) != -1)&&(ack=='a')){
printf("anglee---1---%c\n",ack);
		int totalSize=5;
		totalSize=totalSize-1;
		char *packet=malloc(sizeof(char)*totalSize);
		int numbytes=0;		
		while(numbytes<totalSize){
printf("anglee---2---%d\n",numbytes);
			if(recv(new_fd4,packet+numbytes,totalSize-numbytes,MSG_PEEK)!=-1)
				numbytes=numbytes+recv(new_fd4,packet+numbytes,totalSize-numbytes,0);
			else
				printf("Error a: no new angle data to read\n");			
		}
printf("anglee---3---\n");
		int freenect_angle=atol(packet);
	//printf("Received newAngle=%d\n",freenect_angle);
	printf("Received newanglee=%d_old=%d\n",freenect_angle,freenect_oldAngle);
		if(freenect_oldAngle!=freenect_angle){
			setAngle=1;
			freenect_oldAngle=freenect_angle;
		}
		free(packet);	
	}
//printf("Outside handle angle req_ack=%c\n",ack);
}

void snapshot_accel(freenect_device *dev){
//printf("Inside snapshot_accel\n");
// send accel
	if (!last_timestamp)
		return;
//printf("preparing to SEND:a\n");
	freenect_raw_tilt_state* state;
	freenect_update_tilt_state(dev);
	state = freenect_get_tilt_state(dev);
//printf("current tilt angle=%f\n",freenect_get_tilt_degs(state));	
	accelData[TIMESTAMP_BYTES+CUR_TIME_BYTES+sizeof(*state)]="";
	////printf("--dhere--01\n");
	sprintf( accelData, "%u", last_timestamp );
	char *ct=malloc(CUR_TIME_BYTES);
////printf("--dhere--02_ts=%s\n",ts);
	double cur_time = get_time();
	sprintf( ct, "%f", cur_time );	
	memcpy(accelData+TIMESTAMP_BYTES,ct,CUR_TIME_BYTES);
////printf("--dhere--03_ts=%s_ct=%s\n",ts,ts+10);
	memcpy(accelData+(TIMESTAMP_BYTES+CUR_TIME_BYTES),state,sizeof(*state));
////printf("--dhere--04\n");
	if (send(new_fd4, "a",1, 0) == -1)
		perror("sending Error a signal:accel");
	int sentBytes=send(new_fd1, accelData,accelTotalSize, 0);
	if ( sentBytes== -1)
		perror("sending Error:Depth");
	else if(sentBytes<accelTotalSize){
		while(sentBytes<accelTotalSize){
			sentBytes=sentBytes+send(new_fd1, accelData+sentBytes,accelTotalSize-sentBytes, 0);
		}
	}
	free(ct);
//printf("SENT:accel_%d_timestamp=%u_cur_time=%f\n",totalSize,last_timestamp,cur_time);
// now exchange state info
	handleAngleRequest();
}

void *compress_threadfunc(int compress_flag_back){
//	pthread_mutex_lock(&compress_mutex);
//	sigemptyset(&sigact.sa_mask);
printf("+++++++++++FOUND flag=%d\n",compress_flag_back);
	int rgbpid=fork();
	if(rgbpid==-1)
		perror("Fork error");
//printf("TAG_____________________________ fork cost=%f\n",get_time()-forktime);
	if(rgbpid==0){//child process
		if(compress_flag_back)
			data=&rgbData_back;
		else
			data=&rgbData;
		output=rgb_shared_seg+VIDEO_HEADER_BYTES;
printf("TAG_____________________________ rgb initialising data,output=%d\n",data);
//printf("TAG_____________________________----encoding start frame=%f_%f\n",cur_time,get_time());
double encTime=get_time();
//--
		int argc=10;
		char *argv[argc];
		argv[0]="../ffmpeg-0.7-rc1/./ffmpeg";
		argv[1]="-r";
		argv[2]="30";
		argv[3]="-i";
		argv[4]="r%d.ppm";
		argv[5]="-r";
		argv[6]="30";
		argv[7]="-b";
		argv[8]=RGBBITRATE;
		argv[9]=VIDEOFORMAT;
		num_of_frames_to_encode=FRAMES_TO_ENCODE;
		isDepth=0;
		outSize=0;
		myffmpeg(argc,argv);
		data=data-FREENECT_VIDEO_RGB_SIZE*FRAMES_TO_ENCODE;
//--
printf("TAG_____________________________----rgb video compressed=%d\n",outSize);
		sprintf(rgb_shared_seg,"%u",outSize);
printf("TAG_____________________________ EXITING:rgb_compressed.....##################################_%d\n\n",sizeof(uint16_t));
		exit(0);
	}

	int depthpid=fork();
printf("TAG_____________________________ depth fork done\n");
	if(depthpid==-1)
		perror("Fork error");
//printf("TAG_____________________________ fork cost=%f\n",get_time()-forktime);
	if(depthpid==0){
printf("TAG_____________________________ depth inside_data=%d\n",data);
		if(compress_flag_back)
			data=&depthData_back;
		else
			data=&depthData;
		output=depth_shared_seg+VIDEO_HEADER_BYTES;
printf("TAG_____________________________ depth copied depth=%d\n",data);
//printf("TAG_____________________________----encoding start frame=%f_%f\n",cur_time,get_time());
double encTime=get_time();
//--
		int argc=10;
		char *argv[argc];
		argv[0]="../ffmpeg-0.7-rc1/./ffmpeg";
		argv[1]="-r";
		argv[2]="30";
		argv[3]="-i";
		argv[4]="r%d.pgm";
		argv[5]="-r";
		argv[6]="30";
		argv[7]="-b";
		argv[8]=DEPTHBITRATE;
		argv[9]=VIDEOFORMAT;
		num_of_frames_to_encode=FRAMES_TO_ENCODE;
		outSize=0;
		isDepth=1;
		myffmpeg(argc,argv);
		data=data-FREENECT_DEPTH_11BIT_SIZE*FRAMES_TO_ENCODE;
//--
printf("TAG_____________________________----depth video compressed=%d\n",outSize);
		sprintf(depth_shared_seg,"%u",outSize);
/*		FILE *f;
	  if (!(f=fopen("mynew.mp4","wb")))
	  {
	 printf("Couldn't create output file mynew.mp4");
	    exit(0);
	  }
	//output=output-outSize;
	  int i;
	for(i=0;i<outSize;i++,output++){
		putc(*output,f);
	}
	  fclose(f);

*/
printf("TAG_____________________________ EXITING DEPTH.....##################################\n\n");
		exit(0);
	}
	int statusrgb,statusdepth;
double t=get_time();
printf("TAG_____________________________ waiting for depth compression....\n");
															//TODO:Transfer buf and then wait
	wait(&statusrgb);
printf("TAG_____________________________ wait1 over....%f\n",get_time()-t);
t=get_time();
	wait(&statusdepth);
printf("TAG_____________________________ wait2 over...%f_new_fd3=%d\n",get_time()-t,new_fd3);

	int bytesToSend=FRAMES_TO_ENCODE*(TIMESTAMP_BYTES+CUR_TIME_BYTES);
	int sentBytes;
	if(compress_flag_back){
		sentBytes=send(new_fd3, buf_back,bytesToSend, 0);
		if (sentBytes == -1)
			perror("sending Error:rgb");
		else if(sentBytes<bytesToSend){
			while(sentBytes<bytesToSend){
				sentBytes=sentBytes+send(new_fd3, buf_back+sentBytes,bytesToSend-sentBytes, 0);
			}
		}
	}
	else{
		sentBytes=send(new_fd3, buf,bytesToSend, 0);
		if (sentBytes == -1)
			perror("sending Error:rgb");
		else if(sentBytes<bytesToSend){
			while(sentBytes<bytesToSend){
				sentBytes=sentBytes+send(new_fd3, buf+sentBytes,bytesToSend-sentBytes, 0);
			}
		}
	}
printf("TAG_____________________________ header sent...sentBytes=%d\n",sentBytes);
	unsigned char tmp_video_size[VIDEO_HEADER_BYTES]="";
	memcpy(tmp_video_size,rgb_shared_seg,VIDEO_HEADER_BYTES);
	unsigned int video_size=atol(tmp_video_size);
printf("TAG_____________________________ got video Size=%d\n",video_size);
	bytesToSend=VIDEO_HEADER_BYTES+video_size;
	sentBytes=send(new_fd3, rgb_shared_seg,bytesToSend, 0);
	if (sentBytes == -1)
		perror("sending Error:rgb");
	else if(sentBytes<bytesToSend){
		while(sentBytes<bytesToSend){
			sentBytes=sentBytes+send(new_fd3, rgb_shared_seg+sentBytes,bytesToSend-sentBytes, 0);
		}
	}
printf("TAG_____________________________ Encoded rgb video size=%d\n",video_size);

	tmp_video_size[VIDEO_HEADER_BYTES]="";
	memcpy(tmp_video_size,depth_shared_seg,VIDEO_HEADER_BYTES);
	video_size=atol(tmp_video_size);
	bytesToSend=VIDEO_HEADER_BYTES+video_size;
	sentBytes=send(new_fd3, depth_shared_seg,bytesToSend, 0);
	if (sentBytes == -1)
		perror("sending Error:rgb");
	else if(sentBytes<bytesToSend){
		while(sentBytes<bytesToSend){
			sentBytes=sentBytes+send(new_fd3, depth_shared_seg+sentBytes,bytesToSend-sentBytes, 0);
		}
	}
printf("TAG_____________________________ Encoded depth video size=%d\n",video_size);
//	pthread_mutex_unlock(&compress_mutex);
printf("EXITING THREAD.....##################################\n\n");
	pthread_exit(0);
}
void sync_both(freenect_device *dev, void *frame, uint32_t timestamp, int isDepth){
printf("preparing to SEND:rgbFrameCounter=%d_depthFrameCounter=%d\n",rgbFrameCounter,depthFrameCounter);
		//dump('r', timestamp, rgb, FREENECT_VIDEO_RGB_SIZE);
	if((!isDepth)&& rgbFrameCounter<FRAMES_TO_ENCODE){
		last_timestamp = timestamp;

//printf("--rhere--00\n");
//printf("--rhere--01\n");
		char *ct=malloc(CUR_TIME_BYTES);
//printf("--rhere--02_buf=%s\n",buf);
		double cur_time = get_time();
printf("----------------------------------------------------rhere_cur_time=%f_%g\n",cur_time,cur_time);
		sprintf( ct, "%f", cur_time );
//	strcat(&buf,ct);
//printf("--rhere--03_buf=%s_ct=%s\n",buf+rgbFrameCounter*(TIMESTAMP_BYTES+CUR_TIME_BYTES),buf+rgbFrameCounter*(TIMESTAMP_BYTES+CUR_TIME_BYTES)+TIMESTAMP_BYTES);
//	strcat(&buf,(char*)rgb);
		if(flag_back){
			sprintf( buf_back+rgbFrameCounter*(TIMESTAMP_BYTES+CUR_TIME_BYTES), "%u", timestamp );
			memcpy(buf_back+rgbFrameCounter*(TIMESTAMP_BYTES+CUR_TIME_BYTES)+TIMESTAMP_BYTES,ct,CUR_TIME_BYTES);
			memcpy(rgbData_back+rgbFrameCounter*FREENECT_VIDEO_RGB_SIZE,frame,FREENECT_VIDEO_RGB_SIZE);
		}
		else{
			sprintf( buf+rgbFrameCounter*(TIMESTAMP_BYTES+CUR_TIME_BYTES), "%u", timestamp );
			memcpy(buf+rgbFrameCounter*(TIMESTAMP_BYTES+CUR_TIME_BYTES)+TIMESTAMP_BYTES,ct,CUR_TIME_BYTES);
			memcpy(rgbData+rgbFrameCounter*FREENECT_VIDEO_RGB_SIZE,frame,FREENECT_VIDEO_RGB_SIZE);
		}
		if(rgbFrameCounter==0)
			printf("----first frame=%f_%f\n",cur_time,get_time());
		if(rgbFrameCounter==FRAMES_TO_ENCODE-1)
			printf("----30th frame=%f_%f\n",cur_time,get_time());
		rgbFrameCounter++;
		free(ct);
	}
	else if ( isDepth &&(depthFrameCounter<FRAMES_TO_ENCODE)){
/*if(depthFrameCounter==0){
uint16_t *pframe=(uint16_t *)frame;
int j;
for(j=0;j<FREENECT_DEPTH_11BIT_SIZE/2;j++)
printf("_%d_\n",pframe[j]);
}*/
		if(flag_back)
			memcpy(depthData_back+depthFrameCounter*FREENECT_DEPTH_11BIT_SIZE,frame,FREENECT_DEPTH_11BIT_SIZE);
		else
			memcpy(depthData+depthFrameCounter*FREENECT_DEPTH_11BIT_SIZE,frame,FREENECT_DEPTH_11BIT_SIZE);
		depthFrameCounter++;
	}
	else
		printf("***...Dropping frame to synchronise...***\n");

	if((rgbFrameCounter==FRAMES_TO_ENCODE) && (depthFrameCounter==FRAMES_TO_ENCODE)){
//		assert( (depthFrameCounter>FRAMES_TO_ENCODE));
printf("TAG_____________________________ going to compress thread ...%d_%d\n",rgbFrameCounter,depthFrameCounter);
		double forktime=get_time();
		pthread_t compress_thread;
printf("+++++++%d\n",flag_back);
		int res=pthread_create(&compress_thread,NULL,compress_threadfunc,flag_back);
		if(res){
			printf("pthread_create for rgb failed\n");
			exit(1);
		}
		rgbFrameCounter=0;
		depthFrameCounter=0;
		flag_back=!flag_back;
		//		pthread_join(rgb_thread,NULL);
		//		free(data);
		//		free(output);
printf("TAG_____________________________ compression effect=%f\n",get_time()-forktime);
	}

printf("EXIT_sync_both\n");
}
void depth_cb(freenect_device *dev, void *depth, uint32_t timestamp)
{
	sync_both(dev,depth,timestamp,1);
}
void rgb_cb(freenect_device *dev, void *rgb, uint32_t timestamp)
{
	sync_both(dev,rgb,timestamp,0);
}
void destroy_shm_segs(){
	shmdt(rgb_shared_seg);
	shmdt(depth_shared_seg);
	shmctl(shmidrgb,IPC_RMID,NULL);
	shmctl(shmidDepth,IPC_RMID,NULL);
}
 void signal_handler(int sig){
	if(sig == SIGINT){
		printf("---------------********Caught signal cntrl + c, cleaning up***********--------------\n");
		running = 0;
	}
}
void init_signals(){
	sigact.sa_handler = signal_handler;
	sigemptyset(&sigact.sa_mask);
	sigact.sa_flags = 0;
	sigaction(SIGINT,&sigact,NULL);
}
void init_shm_seg(int *key, char *path,int *shmid,const int size,unsigned char **shared_seg){
	if((*key=ftok(path,'R'))==-1){
		printf("Error key for path=%s",path);
		perror("key not generated");
	}
//printf("TAG_____________________________ keyRgb rgb=%d_size=%d\n",*key,size);
	if((*shmid=shmget(*key,size,0644|IPC_CREAT))==-1){
		printf("Error shmget for path=%s",path);
		printf("Failed to shmget=%s\n",strerror(errno));
	}
	*shared_seg=shmat(*shmid,(void *)0,0);
	if(*shared_seg==(int *)(-1)){
		printf("Error shmat for path=%s",path);
		perror("shmat:shared segment attach failed");
	}
}
void init()
{
////printf("INITIALISING.....\n");
	freenect_context *ctx;
	freenect_device *dev;
	if (freenect_init(&ctx, 0)) {
		printf("Error: Cannot get context\n");
		return;
	}

	if (freenect_open_device(ctx, &dev, 0)) {
		printf("Error: Cannot get device\n");
		return;
	}
	freenect_set_tilt_degs(dev,freenect_oldAngle);
	freenect_set_depth_format(dev, FREENECT_DEPTH_11BIT);
	freenect_start_depth(dev);
	freenect_set_video_format(dev, FREENECT_VIDEO_RGB);
	freenect_start_video(dev);
	freenect_set_depth_callback(dev, depth_cb);
	freenect_set_video_callback(dev, rgb_cb);
	init_signals();

	init_shm_seg(&keyRgb,"../bin/tiltdemo",&shmidrgb,RGB_VIDEO_SIZE,&rgb_shared_seg);
	init_shm_seg(&keyDepth,"../bin/glview",&shmidDepth,DEPTH_VIDEO_SIZE,&depth_shared_seg);

	while (running && freenect_process_events(ctx) >= 0){
		snapshot_accel(dev);
/*		if (accelCount++ >= 2000)
		{
			accelCount = 0;
			freenect_raw_tilt_state* state;
			freenect_update_tilt_state(dev);
			state = freenect_get_tilt_state(dev);
			double dx,dy,dz;
			freenect_get_mks_accel(state, &dx, &dy, &dz);
			//printf("\r raw acceleration: %4d %4d %4d  mks acceleration: %4f %4f %4f", state->accelerometer_x, state->accelerometer_y, state->accelerometer_z, dx, dy, dz);
			fflush(stdout);
		}*/
		if(setAngle==1){
			setAngle=0;			
			freenect_set_tilt_degs(dev,freenect_oldAngle);
printf("......................New angle set\n");
		}	
	}
	destroy_shm_segs();
	freenect_stop_depth(dev);
	freenect_stop_video(dev);
	freenect_close_device(dev);
	freenect_shutdown(ctx);
}
void init_socket(int *sockfd,const char *port,struct addrinfo *hints,struct addrinfo *servinfo,int yes ){
	int rv;
	struct addrinfo *p;
	if ((rv = getaddrinfo(NULL, port, hints, &servinfo)) != 0) {
		fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
		return 1;
	}
	// loop through all the results and bind to the first we can
	for(p = servinfo; p != NULL; p = p->ai_next) {
		if ((*sockfd = socket(p->ai_family, p->ai_socktype,p->ai_protocol)) == -1) {
			perror("server: socket");
			continue;
		}
		if (setsockopt(*sockfd, SOL_SOCKET, SO_REUSEADDR, &yes,sizeof(int)) == -1) {
			perror("setsockopt");
			exit(1);
		}
		if (bind(*sockfd, p->ai_addr, p->ai_addrlen) == -1){
			close(*sockfd);
			perror("server: bind");
			continue;
		}
		break;
	}
	if (p == NULL)  {
		fprintf(stderr, "server: failed to bind\n");
		return 2;
	}
	if (listen(*sockfd, BACKLOG) == -1) {
		perror("listen");
		exit(1);
	}
}
void close_sockets(int *sockfd1,int *sockfd2,int *sockfd3, int* sockfd4){
	close(*sockfd1); // child doesn't need the listener
	close(*sockfd2);
	close(*sockfd3);
	close(*sockfd4);
}

int main(int argc, char **argv)
{
	struct addrinfo hints, *servinfo, *p;
	struct sockaddr_storage their_addr; // connector's address information
	socklen_t sin_size;
	struct sigaction sa;
	char s[INET6_ADDRSTRLEN];
	int rv;
////////////////////////////////////////////////////////////////////////////////////////
	memset(&hints, 0, sizeof hints);
	hints.ai_family = AF_UNSPEC;
	hints.ai_socktype = SOCK_STREAM;
	hints.ai_flags = AI_PASSIVE; // use my IP
	init_socket(&sockfd1,PORT1,&hints,servinfo,1 );
	init_socket(&sockfd2,PORT2,&hints,servinfo,1 );
	init_socket(&sockfd3,PORT3,&hints,servinfo,1 );
	init_socket(&sockfd4,PORT4,&hints,servinfo,1 );

	freeaddrinfo(servinfo); // all done with this structure

/*	sa.sa_handler = sigchld_handler; // reap all dead processes
	sigemptyset(&sa.sa_mask);
	sa.sa_flags = SA_RESTART;
	if (sigaction(SIGCHLD, &sa, NULL) == -1) {
		perror("sigaction");
		exit(1);
	}
*/
///////////////////////////////////////////////////////////////////////////	
	//printf("server: waiting for connections...\n");

	while(1) {  // main accept() loop
		sin_size = sizeof their_addr;
		new_fd1 = accept(sockfd1, (struct sockaddr *)&their_addr, &sin_size);
		new_fd2 = accept(sockfd2, (struct sockaddr *)&their_addr, &sin_size);
		new_fd3 = accept(sockfd3, (struct sockaddr *)&their_addr, &sin_size);
		new_fd4 = accept(sockfd4, (struct sockaddr *)&their_addr, &sin_size);
//fcntl(new_fd4,F_SETFL,fcntl(new_fd4,F_GETFL,0)|O_NONBLOCK);		
		if (new_fd1 == -1 || new_fd2==-1|| new_fd3==-1|| new_fd4==-1) {
			perror("accept");
			continue;
		}

		inet_ntop(their_addr.ss_family,
			get_in_addr((struct sockaddr *)&their_addr),
			s, sizeof s);
		//printf("server: got connection from %s\n", s);

		if (!fork()) { // this is the child process
			close_sockets(&sockfd1,&sockfd2,&sockfd3,&sockfd4);// child doesn't need the listener
			init();
			close_sockets(&new_fd1,&new_fd2,&new_fd3,&new_fd4);
			exit(0);
		}
		close_sockets(&new_fd1,&new_fd2,&new_fd3,&new_fd4);// parent doesn't need this
	}
	return 0;
}
